#ifndef REC_H
#define REC_H

#include <ros/ros.h>
#include <vector>
using namespace std;

class PoseInternal
{
public:
	PoseInternal();
	PoseInternal(double x, double y, double z,
				 double rx, double ry, double rz, double rw,
				 ros::Time timestamp);

	double mX, mY, mZ;
	double mRx, mRy, mRz, mRw;

	enum GripperState
	{
		OPEN,
		CLOSED,
		LIMBO
	};

	int mGripper;

	ros::Time mTimestamp;
	ros::Duration mOffset;
};

class Recording
{
public:
	Recording(int id = -1);

	// Save a recording to a file
	void Save(string filename);
	// Loads a recording from a file. Throws a string exception if file is not found
	void Load(string filename);

	bool operator==(int id) { return id == objectID; }

	void HandlePose(const PoseInternal &pose);
	int ID() const { return objectID; }
	size_t numPoses() const { return mPoses.size(); }
	vector<PoseInternal>& Poses() { return mPoses; }

	ros::Duration GetMaxTime() const
	{
		if(mPoses.size())
			return mPoses.back().mOffset;
		else
			return ros::Duration(0);
	}
protected:
	int objectID;
	vector<PoseInternal> mPoses;
};

class PlaybackInterface
{
public:
	PlaybackInterface(Recording* rec);

	// Updates the interface with the time that has passed and returns any points that should be emitted
	vector<PoseInternal> Update(ros::Duration dt);

	ros::Duration GetPlaybackTimer() const {return mPlaybackTimer; }
	Recording* GetRecording() const { return mRecording; }
	bool AtEnd() const { return mLastEmittedPose + 1 >= (int)mRecording->numPoses(); }
	int LastPose() const { return mLastEmittedPose; }
private:
	ros::Duration mPlaybackTimer;
	Recording* mRecording;
	int mLastEmittedPose;
};

class RecordController
{
public:
	RecordController(ros::NodeHandle& node);

	enum State
	{
		STANDBY,
		RECORDING,
		PLAYBACK
	};

	void Play(Recording* rec);

	void Update(ros::Duration dt);
	void HandlePose(const PoseInternal &pose, int objectID);
	vector<Recording*>& GetRecordings() { return mRecordings; }
	vector<Recording*>& GetPassiveRecordings() { return mPassiveRecordings; }

	int mState;

	ros::Publisher mPlaybackPub;
	ros::Publisher mPlaybackGripperPub;

	PlaybackInterface* mPlaybackInterface;
protected:
	vector<Recording*> mRecordings;
	vector<Recording*> mPassiveRecordings;
};

#endif // REC_H
